//
// Created by 29245 on 2025/9/1.
//

#include "UI.h"
#include "PS2.h"
#include "action.h"
#include "stdio.h"
#include "oled.h"
#include "font.h"
#include "stdlib.h"
#include "delay.h"
#include "main.h"


extern PS2_TypeDef ps2;

typedef enum {
    JOYSTICK_CONTROL_NONE,          // 无摇杆控制
    JOYSTICK_CONTROL_LEFT,          // 左摇杆控制（行走）
    JOYSTICK_CONTROL_RIGHT          // 右摇杆控制（旋转）
} joystick_control_state_t;

// 添加摇杆模式下的子状态枚举，共8个状态
typedef enum {
    TEST_ONE,            // 第一个子状态（原高度控制）
    TEST_SECOND,         // 第二个子状态
    TEST_THIRD,          // 第三个子状态
    TEST_FOURTH,         // 第四个子状态
    TEST_FIFTH,          // 第五个子状态
    TEST_SIXTH,          // 第六个子状态
    TEST_SEVENTH,        // 第七个子状态
    TEST_EIGHTH,          // 第八个子状态
    TEST_NINE,           // 第九个子状态
    TEST_TEN,            // 第十个子状态
    TEST_ELEVEN,         // 第十一个子状态
    TEST_TWELFTH,        // 第十二个子状态
    TEST_BASE,           // 基础状态
} joystick_sub_state_t;


void UI_Process(void) {
    static uint8_t last_PS2_BTN1 = 0;  // 上一次按键状态
    static uint8_t last_PS2_BTN2 = 0;  // 上一次按键状态
    static uint8_t control_enabled = 0; // 控制模式启用标志
    static int last_PS2_RX = 0;
    static int last_PS2_LX = 0;
    static int last_PS2_LY = 0;
    static int last_PS2_RY = 0;
    static joystick_control_state_t joystick_control_state = JOYSTICK_CONTROL_NONE;  // 摇杆控制状态机
    static joystick_sub_state_t joystick_sub_state = TEST_BASE;  // 摇杆模式下的子状态



    // 按下K1键（PS2_KEY == 1）切换控制模式启用状态
    if (ps2.btn1 == 1 && last_PS2_BTN1 != 1) {

        control_enabled = !control_enabled;  // 切换控制模式启用状态
        if (!control_enabled) {
          OLED_NewFrame();
          // OLED_DrawImage(30, -20, &终极蜘蛛侠Img, OLED_COLOR_REVERSED);
          OLED_PrintString(15, 32, "按下SELECT", &font15x15, OLED_COLOR_NORMAL);
          OLED_PrintString(15, 50, "进入控制模式", &font15x15, OLED_COLOR_NORMAL);
          OLED_ShowFrame();
            // 如果关闭控制模式，确保机器人停止并重置控制模式
            action_stop_continuous();
            action_turn_stop();
            action_set_control_mode(CONTROL_MODE_IDLE);
            joystick_control_state = JOYSTICK_CONTROL_NONE;  // 重置摇杆控制状态
            joystick_sub_state = TEST_BASE;  // 重置子状态
        } else {
            // 进入控制模式，默认为摇杆控制模式
          OLED_PrintString(15, 32, "摇杆模式已开启", &font15x15, OLED_COLOR_NORMAL);
          OLED_PrintString(15, 50, "按下MODE切换模式", &font15x15, OLED_COLOR_NORMAL);
          OLED_ShowFrame();
            action_set_control_mode(CONTROL_MODE_JOYSTICK);
        }
    }

    // 只有当控制模式启用时，才更新控制模式状态机
    if (control_enabled) {
        // 更新控制模式状态机
        action_update_control_mode(ps2.mode, PS2_LX, PS2_LY);
        // 根据当前控制模式执行相应的控制逻辑
        switch (action_get_control_mode()) {
          case CONTROL_MODE_BUTTON:

            if (ps2.mode == 65) {
              // 检测K5按键（向前移动）
              if (ps2.btn1 ==  16 && last_PS2_BTN1 != 16) {
                action_move_to_direction(1.0f, 0.0f, 0);  // 向前无限移动
              } else if (ps2.btn1 !=  16 && last_PS2_BTN1 == 16) {
                action_stop_continuous();  // 释放时停止
              }
              else if (ps2.btn1 == 64 && last_PS2_BTN1 != 64) {
                action_move_to_direction(-1.0f, 0.0f, 0);  // 向后移动
              } else if (ps2.btn1 != 64 && last_PS2_BTN1 == 64) {
                action_stop_continuous();  // 释放时停止
              }
              else if (ps2.btn1 == 32 && last_PS2_BTN1 != 32) {
                action_move_to_direction(0.0f, -1.0f, 0);  // 向右移动
              } else if (ps2.btn1 != 32 && last_PS2_BTN1 == 32) {
                action_stop_continuous();  // 释放时停止
              }
              else if (ps2.btn1 == 128 && last_PS2_BTN1 != 128) {
                action_move_to_direction(0.0f, 1.0f, 0);  // 向左移动
              } else if (ps2.btn1 != 128 && last_PS2_BTN1 == 128) {
                action_stop_continuous();  // 释放时停止
              }
              // 添加K10键控制顺时针旋转
              else if (ps2.btn2 == 2 && last_PS2_BTN2 != 2) {
                action_turn_right_start();  // 启动顺时针旋转
              } else if (ps2.btn2 != 2 && last_PS2_BTN2 == 2) {
                action_turn_stop();  // 释放时停止旋转
              }
              // 添加K9键控制逆时针旋转
              else if (ps2.btn2 == 1 && last_PS2_BTN2 != 1) {
                action_turn_left_start();  // 启动逆时针旋转
              } else if (ps2.btn2 != 1 && last_PS2_BTN2 == 1) {
                action_turn_stop();  // 释放时停止旋转
              }

            }
            break;

          case CONTROL_MODE_JOYSTICK:
            // 实现摇杆控制状态机
            // 检查是否有新的输入
            uint8_t left_joystick_active = (abs(PS2_LX) > 10 || abs(PS2_LY) > 10);
            uint8_t right_joystick_active = (abs(PS2_RX) > 10);

            if (ps2.btn1 == 16) {
              joystick_sub_state = TEST_ONE;       // btn1=16时对应TEST_ONE
            } else if (ps2.btn1 == 32) {
              joystick_sub_state = TEST_SECOND;    // btn1=32时对应TEST_SECOND
            } else if (ps2.btn1 == 64) {
              joystick_sub_state = TEST_THIRD;     // btn1=64时对应TEST_THIRD
            } else if (ps2.btn1 == 128) {
              joystick_sub_state = TEST_FOURTH;    // btn1=128时对应TEST_FOURTH
            } else if (ps2.btn2 == 1) {
              joystick_sub_state = TEST_FIFTH;     // btn2=1时对应TEST_FIFTH
            } else if (ps2.btn2 == 2) {
              joystick_sub_state = TEST_SIXTH;     // btn2=2时对应TEST_SIXTH
            } else if (ps2.btn2 == 4) {
              joystick_sub_state = TEST_SEVENTH;   // btn2=4时对应TEST_SEVENTH
            } else if (ps2.btn2 == 8) {
              joystick_sub_state = TEST_EIGHTH;    // btn2=8时对应TEST_EIGHTH
            } else if (ps2.btn2 == 16) {
              joystick_sub_state = TEST_NINE;      // btn2=16时对应TEST_NINE
            } else if (ps2.btn2 == 32) {
              joystick_sub_state = TEST_TEN;       // btn2=32时对应TEST_TEN
            } else if (ps2.btn2 == 64) {
              joystick_sub_state = TEST_ELEVEN;    // btn2=64时对应TEST_ELEVEN
            } else if (ps2.btn2 == 128) {
              joystick_sub_state = TEST_TWELFTH;   // btn2=128时对应TEST_TWELFTH
            } else if (joystick_sub_state != TEST_BASE &&
                      ps2.btn1 != 16 && ps2.btn1 != 32 && ps2.btn1 != 64 && ps2.btn1 != 128 &&
                      ps2.btn2 != 1 && ps2.btn2 != 2 && ps2.btn2 != 4 && ps2.btn2 != 8 &&
                      ps2.btn2 != 16 && ps2.btn2 != 32 && ps2.btn2 != 64 && ps2.btn2 != 128) {
              // 松开按键时返回基础状态
              joystick_sub_state = TEST_BASE;
              // 重置摇杆控制状态机，确保回到正常控制状态
              joystick_control_state = JOYSTICK_CONTROL_NONE;
                      }
            switch (joystick_sub_state) {
              case TEST_ONE:
                // ps2.btn1=16时的动作
                action_squat_stand(1, 30);
                break;
              case TEST_SECOND:
                // ps2.btn1=32时的动作
                action_jump(1, 50);
                break;
              case TEST_THIRD:
                // ps2.btn1=64时的动作
                action_body_sway(1, 40, 30);
                break;
              case TEST_FOURTH:
                // ps2.btn1=128时的动作
                action_body_sway(0, 40, 30);
                break;
              case TEST_FIFTH:
                //ps2.btn2=1时的动作
                action_body_twist(-15.0f, 30);
                break;
              case TEST_SIXTH:
                //ps2.btn2=2时的动作
                action_body_twist(15.0f, 30);
                break;

              case TEST_SEVENTH:
                //ps2.btn2=4时的动作
                action_body_wave(18, 30);
                break;

              case TEST_EIGHTH:
                //ps2.btn2=8时的动作
                 action_nod_head(1, 30);
                break;
              case TEST_NINE:
                //ps2.btn2=16时的动作
                action_wave_hand(1, 30);
                break;
              case TEST_TEN:
                //ps2.btn2=32时的动作
                action_wave_dance(1, 24);
                break;
              case TEST_ELEVEN:
                //ps2.btn2=64时的动作
                 action_robot_dance(3, 30);
                break;
              case TEST_TWELFTH:
                //ps2.btn2=128时的动作
                action_disco_dance(1, 30);
                break;

              case TEST_BASE:
              default:
                // 默认状态，执行标准摇杆控制逻辑
                // 根据状态机决定执行哪个控制
                switch (joystick_control_state) {
                  case JOYSTICK_CONTROL_NONE:
                    // 无控制状态，检查哪个摇杆先有输入
                    if (left_joystick_active) {
                      joystick_control_state = JOYSTICK_CONTROL_LEFT;
                      action_move_by_joystick(PS2_LY, PS2_LX);
                    } else if (right_joystick_active) {
                      joystick_control_state = JOYSTICK_CONTROL_RIGHT;
                      if (PS2_RX > 0) {
                        action_turn_left_start();
                      } else {
                        action_turn_right_start();
                      }
                    }
                    break;

                  case JOYSTICK_CONTROL_LEFT:
                    // 左摇杆控制状态（行走）
                    if (!left_joystick_active) {
                      // 左摇杆回到中心，停止移动
                      action_stop_continuous();
                      joystick_control_state = JOYSTICK_CONTROL_NONE;
                    } else if (right_joystick_active) {
                      // 右摇杆有输入，切换到右摇杆控制
                      action_stop_continuous();
                      joystick_control_state = JOYSTICK_CONTROL_RIGHT;
                      if (PS2_RX > 0) {
                        action_turn_left_start();
                      } else {
                        action_turn_right_start();
                      }
                    } else {
                      // 继续左摇杆控制
                      action_move_by_joystick(PS2_LY, PS2_LX);
                    }
                    break;
                  case JOYSTICK_CONTROL_RIGHT:
                    // 右摇杆控制状态
                    if (!right_joystick_active) {
                      // 右摇杆回到中心，停止旋转
                      action_turn_stop();
                      joystick_control_state = JOYSTICK_CONTROL_NONE;
                    } else if (left_joystick_active) {
                      // 左摇杆有输入，切换到左摇杆控制
                      action_turn_stop();
                      joystick_control_state = JOYSTICK_CONTROL_LEFT;
                      action_move_by_joystick(PS2_LY, PS2_LX);
                    } else {
                      // 继续右摇杆控制
                      if (PS2_RX > 0 && last_PS2_RX <= 0) {
                        action_turn_left_start();
                      } else if (PS2_RX < 0 && last_PS2_RX >= 0) {
                        action_turn_right_start();
                      } else if (PS2_RX == 0) {
                        action_turn_stop();
                      }
                    }
                    break;
                }
                break;
            }
            break;

          case CONTROL_MODE_IDLE:
          default:
            // 空闲状态下确保机器人停止
            action_stop_continuous();
            action_turn_stop();
            joystick_control_state = JOYSTICK_CONTROL_NONE;  // 重置摇杆控制状态
            joystick_sub_state = TEST_BASE;  // 重置子状态
            break;
        }
    } else {
        // 当控制未启用时，确保机器人处于空闲状态
        action_stop_continuous();
        action_turn_stop();
    }
    action_move_execute();
    last_PS2_BTN1 = ps2.btn1;  // 保存当前按键状态
    last_PS2_BTN2 = ps2.btn2;  // 保存当前按键状态
    last_PS2_RX = PS2_RX;
    last_PS2_LX = PS2_LX;
    last_PS2_LY = PS2_LY;
    last_PS2_RY = PS2_RY;
  // printf("MODE:%d BTN1:%d BTN2:%d R_LR:%d R_UD:%d L_LR:%d L_UD:%d\r\n",
  // ps2.mode, ps2.btn1, ps2.btn2,
  // PS2_RX, PS2_RY, PS2_LX, PS2_LY);

}
